﻿using UnityEngine;

namespace Panda.Utils
{
    public static class MathUtil
    {
        public static bool Vector3Similar(Vector3 a, Vector3 b, float epsilon = 0.0001f)
        {
            if (FloatSimilar(a.x, b.x, epsilon) == false) return false;
            if (FloatSimilar(a.y, b.y, epsilon) == false) return false;
            if (FloatSimilar(a.z, b.z, epsilon) == false) return false;

            return true;
        }

        public static bool Vector2Similar(Vector2 a, Vector2 b, float epsilon = 0.0001f)
        {
            if (FloatSimilar(a.x, b.x, epsilon) == false) return false;
            if (FloatSimilar(a.y, b.y, epsilon) == false) return false;

            return true;
        }

        public static bool QuaternionAngleSimilar(Quaternion a, Quaternion b, bool justComponent, float angleThreshold = 10f)
        {
            if (justComponent)
            {
                if (FloatSimilar(a.x, b.x) == false)  return false;
                if (FloatSimilar(a.y, b.y) == false)  return false;
                if (FloatSimilar(a.z, b.z) == false)  return false;
                if (FloatSimilar(a.w, b.w) == false)  return false;
                
                return true;
            }
            
            Vector3 eulerAngles1 = a.eulerAngles;
            Vector3 eulerAngles2 = b.eulerAngles;
            
            Vector3 angleDifference = eulerAngles1 - eulerAngles2;
            angleDifference.x = Mathf.DeltaAngle(eulerAngles1.x, eulerAngles2.x);
            angleDifference.y = Mathf.DeltaAngle(eulerAngles1.y, eulerAngles2.y);
            angleDifference.z = Mathf.DeltaAngle(eulerAngles1.z, eulerAngles2.z);

            bool isSimilar = Mathf.Abs(angleDifference.x) <= angleThreshold &&
                             Mathf.Abs(angleDifference.y) <= angleThreshold &&
                             Mathf.Abs(angleDifference.z) <= angleThreshold;

            return isSimilar;
        }

        public static bool FloatSimilar(float a, float b, float epsilon = 0.0001f)
        {
            if (Mathf.Abs(a - b) > epsilon)
            {
                return false;
            }
            
            return true;
        }

        public static bool ColorSimilar(Color a, Color b, float epsilon = 0.0001f)
        {
            if (FloatSimilar(a.r, b.r, epsilon) == false)  return false;
            if (FloatSimilar(a.g, b.g, epsilon) == false)  return false;
            if (FloatSimilar(a.b, b.b, epsilon) == false)  return false;
            if (FloatSimilar(a.a, b.a, epsilon) == false)  return false;

            return true;
        }
    }
}